package FRC2856.Robot;

import edu.wpi.first.wpilibj.Servo;
/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */


import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.image.*;
import edu.wpi.first.wpilibj.util.SortedVector;


/**
 *
 * @author Dustin
 */


public class CustomCamera {
    Servo serv;
    public AxisCamera camera;
    CriteriaCollection cc, cc2;
    Relay LED; //turning on and off the LED
    boolean LEDOn = false; //if the LED is on or off
    NewNIVision nniv = new NewNIVision();
    
    public CustomCamera(){
        serv = new Servo(RobotMap.cameraServoModule, RobotMap.cameraServoChannel);
        camera = AxisCamera.getInstance(RobotMap.cameraIP);
        LED = new Relay(RobotMap.lightRelayModule, RobotMap.lightRelayPort, Relay.Direction.kForward);
        cc = new CriteriaCollection();      // create the criteria for the particle filter
        cc.addCriteria(NIVision.MeasurementType.IMAQ_MT_BOUNDING_RECT_WIDTH, 30, 400, false);
        cc.addCriteria(NIVision.MeasurementType.IMAQ_MT_BOUNDING_RECT_HEIGHT, 40, 400, false);
        
        cc2 = new CriteriaCollection();
        cc2.addCriteria(NIVision.MeasurementType.IMAQ_MT_CONVEX_HULL_PERIMETER, 75, 500, false);
    }
    
    public ParticleAnalysisReport getLowGoal(){
        ParticleAnalysisReport[] reports = getParticles();
        ParticleAnalysisReport toReturn = null;
        double lowY;
        if(reports.length<1)
            return null;
        toReturn = reports[0];
        lowY = reports[0].center_mass_y;
        for(int i = 1; i<reports.length; i++){
            if(reports[i].center_mass_y>lowY){
                lowY = reports[i].center_mass_y;
                toReturn = reports[i];
            }
        }
        return toReturn;
    }
    

    
    private ParticleAnalysisReport[] getParticles(){
        ParticleAnalysisReport[] reports = new ParticleAnalysisReport[0];
        ColorImage[] images = takePictures(true);
         try {
                /**
                 * Do the image capture with the camera and apply the algorithm described above. This
                 * sample will either get images from the camera or from an image file stored in the top
                 * level directory in the flash memory on the cRIO. The file name in this case is "10ft2.jpg"
                 * 
                 */
                ColorImage image1 = null;
                ColorImage image2 = null;
                 
                image1 = images[0]; // comment if using stored images
                image2 = images[1];
                ColorImage subtractedImage = camera.getImage();
                
                nniv.subtract(subtractedImage.image, image2.image, image1.image);
                
                //ColorImage image;                           // next 2 lines read image from flash on cRIO
                //image =  new RGBImage("/10ft2.jpg");
                BinaryImage thresholdImage = subtractedImage.thresholdRGB(25, 255, 0, 45, 0, 47);   // keep only red objects
                BinaryImage bigObjectsImage = thresholdImage.removeSmallObjects(false, 2);  // remove small artifacts
                BinaryImage convexHullImage = bigObjectsImage.convexHull(false);          // fill in occluded rectangles
                BinaryImage filteredImage = convexHullImage.particleFilter(cc);           // find filled in rectangles
                reports = filteredImage.getOrderedParticleAnalysisReports();  // get list of results
                /**
                 * all images in Java must be freed after they are used since they are allocated out
                 * of C data structures. Not calling free() will cause the memory to accumulate over
                 * each pass of this loop.
                 */
                filteredImage.free();
                convexHullImage.free();
                bigObjectsImage.free();
                thresholdImage.free();
                image1.free();
                image2.free();
                subtractedImage.free();
                
            } catch (NIVisionException ex) {
                ex.printStackTrace();
            } catch (AxisCameraException ex) {
                ex.printStackTrace();
            }
         return reports;
    }
    
    
    
    public ParticleAnalysisReport[] getParticles2(){
        
        ParticleAnalysisReport[] reports = new ParticleAnalysisReport[0];
        ColorImage[] images = takePictures(true);
         try {
                /**
                 * Do the image capture with the camera and apply the algorithm described above. This
                 * sample will either get images from the camera or from an image file stored in the top
                 * level directory in the flash memory on the cRIO. The file name in this case is "10ft2.jpg"
                 * 
                 */
                ColorImage image1 = null;
                ColorImage image2 = null;
                 
                image1 = images[0]; // comment if using stored images
                image2 = images[1];
                ColorImage subtractedImage = camera.getImage();
                
                nniv.subtract(subtractedImage.image, image2.image, image1.image);
                MonoImage lumPlane = subtractedImage.getLuminancePlane();
                MonoImage threshImage = new MonoImage();
                nniv.threshold(threshImage.image, lumPlane.image, 50, 255);
                BinaryImage threshBinImage = (BinaryImage)threshImage;
                MonoImage filteredImage = new MonoImage();
                nniv.particleFilter(filteredImage.image, threshBinImage.image, cc2);
                BinaryImage filteredBinImage = (BinaryImage)filteredImage;
                reports = filteredBinImage.getOrderedParticleAnalysisReports();
                
                image1.free();
                image2.free();
                subtractedImage.free();
                lumPlane.free();
                threshImage.free();
                threshBinImage.free();
                filteredImage.free();
                filteredBinImage.free();
                
            } catch (NIVisionException ex) {
                ex.printStackTrace();
            } catch (AxisCameraException ex) {
                ex.printStackTrace();
            }
         return reports;
    }
    
    
    
    
    
    /**
     * Preconditions: give it a boolean b
     * Postconditions: It will return an array of ColorImages taken by the camera with first index an image with the LED
     * off; if b is true than the array will also contain a second image with the LED on. At the end of the function, the LED
     * status will be the same as at the beginning of the function
     */
    public ColorImage[] takePictures(boolean b){
        ColorImage[] toReturn; //get the Arrat you will return later declared and instantiated
        if(b)
            toReturn = new ColorImage[2];
        else
            toReturn = new ColorImage[1];
        
        LED.set(Relay.Value.kOff); //set LED off
        
        try{
            toReturn[0] = camera.getImage(); //take an image without LED
        }
        catch(Exception e){
            e.printStackTrace();
        }
        
        if(b){ //if an image with LED on is also desired
            LED.set(Relay.Value.kOn); //turn LED on
            try{
                toReturn[1] = camera.getImage(); //take an image with LED
            }
            catch(Exception e){
             e.printStackTrace();
            }
        }
        
        setLEDStatus(getLEDStatus()); //fix the LED status to what it should be.
        
        return toReturn; //return the needed Array
    }
    
    public boolean getLEDStatus(){ //getting if the LED is on or off
        return LEDOn;
    }
    
    public void setLEDStatus(boolean b){ //setting the LED on or off
        LEDOn = b; //save b
        if(b) //set on or off based on B
            LED.set(Relay.Value.kOn);
        else
            LED.set(Relay.Value.kOff);
    }
    
    public int getNumberParticles(Image i){
        try {
            return NewNIVision.countParticles(i.image);
        } catch (NIVisionException ex) {
            ex.printStackTrace();
        }
        return 0;
    }
    
    public void rotateServo(double amount){
        double servoAngle = serv.get();
        if(servoAngle+amount>1)
            serv.set(1);
        else if(servoAngle+amount<0)
            serv.set(0);
        else
            serv.set(amount+servoAngle);
    }
    
    /**
     * Gets all the particle analysis reports ordered from largest area to smallest.
     * @param size The number of particles to return
     * @return An array of ParticleReports from largest area to smallest
     */
   /* public ParticleAnalysisReport[] getOrderedParticleAnalysisReports(Image i, int size) throws NIVisionException{
        BinaryImage.ParticleSizeReport[] reports = new BinaryImage.ParticleSizeReport[size];
        SortedVector sorter = new SortedVector(new SortedVector.Comparator() {
            public int compare(Object object1, Object object2) {
                BinaryImage.ParticleSizeReport p1 = (BinaryImage.ParticleSizeReport)object1;
                BinaryImage.ParticleSizeReport p2 = (BinaryImage.ParticleSizeReport)object2;
                if (p1.size < p2.size)
                    return -1;
                else if (p1.size > p2.size)
                    return 1;
                return 0;
            }
        });
        for (int i = 0; i < getNumberParticles(); i++)
            sorter.addElement(new BinaryImage.ParticleSizeReport(i));
        sorter.setSize(size);
        sorter.copyInto(reports);
        ParticleAnalysisReport[] finalReports = new ParticleAnalysisReport[reports.length];
        for (int i = 0; i < finalReports.length; i++)
            finalReports[i] = reports[i].getParticleAnalysisReport();
        return finalReports;
    }

    /**
     * Gets all the particle analysis reports ordered from largest area to smallest.
     * @return An array of ParticleReports from largest are to smallest
     *//*
    public ParticleAnalysisReport[] getOrderedParticleAnalysisReports(Image i) throws NIVisionException{
        return getOrderedParticleAnalysisReports(i, getNumberParticles(i));
    }*/
    
}
